#ifndef __LOGIC
#define __LOGIC
#include "main.hpp"
#include "move.hpp"
#include "graph.hpp"
extern Vision* major_Vision;
extern Vision* sub_Vision;
extern base_controller* major_Controller;
extern std::function<void()> func3;

enum Stats{
	CORRECTING_FRAME_POSITION,
	CORRECTING_FRAME_ORIEN,
	GOING_DIRECT_THROUGH,
	COLLIDING_BALL,
	TRACKING_LANDMARK,
	MIDDLE_STATE
};

//增量式pid
class PID_incre
{
public:
	PID_incre(float p,float i,float d,float oMa,float oMi)
		:P(p),I(i),D(d),outMax(oMa),outMin(oMi)
	{
		error_1 = 0;
		error_2 = 0;
	}
	float update(float target,float measure)
	{
		float error=measure-target;
		out+=P*(error-error_1)+I*(error)+D*(error-2*error_1+error_2);
		out = out>outMax?outMax:out;
		out = out<outMin?outMin:out;
		error_1 = error;
		error_2 = error_1;
		return this->out;
	}
private:
	float P;
	float I;
	float D;
	float error_1;
	float error_2;
	float outMax,outMin;
public:
	float out;
};

//简单一维卡尔曼滤波
class SimpleKF
{
public:
	float X;
	float Q,R,P;
	float update(float observe)//observe --> Y
	{//default: A=1,B=0; C=1;
		float K,X_,P_;
		X_=X;
		P_= P+Q;
		K = P_/(P_+R);
		X = X_+K*(observe-X_);
		P=(1-K)*P_;
		return X;
	}
	SimpleKF(float pre,float q,float r)
	:Q(q),R(r),X(pre){
		this->P=0;
	}
};

#endif // !__LOGIC